import paho.mqtt.client as mqtt
import json
import time
import uuid # 用于生成唯一的请求ID

# --- MQTT 配置 ---
MQTT_BROKER_HOST = "118.89.105.38" 
MQTT_BROKER_PORT = 1883
MQTT_USERNAME = "buaauav" 
MQTT_PASSWORD = "buaauav"

# MQTT 话题定义
SIMULATED_ROS_TOPIC = "drone/trajectory/setpoint_raw" # 订阅本机发布的模拟航迹数据
COMMAND_TOPIC = "drone/command/trigger_algo" # 用于发布指令到本机

CLIENT_ID = "remote_console_subscriber_001"

# --- MQTT 回调函数 ---
def on_connect(client, userdata, flags, rc):
    if rc == 0:
        print(f"MQTT Connected to Broker ({MQTT_BROKER_HOST})!")
        client.subscribe(SIMULATED_ROS_TOPIC) # 订阅模拟航迹数据话题
        print(f"Subscribed to trajectory topic: {SIMULATED_ROS_TOPIC}")
    else:
        print(f"MQTT Failed to connect, return code {rc}")
        exit(1)

def on_message(client, userdata, msg):
    try:
        payload = json.loads(msg.payload.decode())
        # 模拟 rostopic echo 打印格式
        print("\n------------------------------------------")
        print(f"Topic: {msg.topic}")
        print(f"  request_id: {payload.get('request_id', 'N/A')}")
        
        header = payload.get('header', {})
        print(f"  header:")
        print(f"    stamp: {header.get('stamp', 'N/A')}")
        print(f"    frame_id: {header.get('frame_id', 'N/A')}")
        
        print(f"  coordinate_frame: {payload.get('coordinate_frame', 'N/A')}")
        print(f"  type_mask: {payload.get('type_mask', 'N/A')}")
        print(f"  x: {payload.get('x', 'N/A'):.4f}") # 格式化输出浮点数
        print(f"  y: {payload.get('y', 'N/A'):.4f}")
        print(f"  z: {payload.get('z', 'N/A'):.4f}")
        print(f"  vx: {payload.get('vx', 'N/A'):.4f}")
        print(f"  vy: {payload.get('vy', 'N/A'):.4f}")
        print(f"  vz: {payload.get('vz', 'N/A'):.4f}")
        print(f"  afx: {payload.get('afx', 'N/A'):.4f}")
        print(f"  afy: {payload.get('afy', 'N/A'):.4f}")
        print(f"  afz: {payload.get('afz', 'N/A'):.4f}")
        print(f"  yaw: {payload.get('yaw', 'N/A'):.4f}")
        print(f"  yaw_rate: {payload.get('yaw_rate', 'N/A'):.4f}")
        print("------------------------------------------")
    except json.JSONDecodeError:
        print(f"Received non-JSON message on {msg.topic}: {msg.payload.decode()}")
    except Exception as e:
        print(f"Error processing MQTT message: {e}")

# --- 主程序逻辑 ---
if __name__ == "__main__":
    # 初始化 MQTT 客户端
    client = mqtt.Client(client_id=CLIENT_ID)
    if MQTT_USERNAME and MQTT_PASSWORD:
        client.username_pw_set(MQTT_USERNAME, MQTT_PASSWORD) # 设置用户名和密码

    client.on_connect = on_connect
    client.on_message = on_message

    try:
        client.connect(MQTT_BROKER_HOST, MQTT_BROKER_PORT, 60)
        client.loop_start() # 在后台线程运行，处理消息收发
    except Exception as e:
        print(f"Failed to connect to MQTT broker: {e}")
        exit(1)

    print("\n--- Remote Console Subscriber Started ---")
    print("Type 'g' to send 'generate_trajectory' command.")
    print("Type 's' to send 'stop_trajectory' command.")
    print("Type 'q' to quit.")

    while True:
        cmd = input("> ").strip().lower()

        if cmd == 'q':
            break
        elif cmd == 'g':
            req_id = str(uuid.uuid4())
            command_payload = {
                "request_id": req_id,
                "command": "generate_trajectory",
                "parameters": {"duration": 10, "altitude": 5.0} # 示例参数
            }
            client.publish(COMMAND_TOPIC, json.dumps(command_payload), qos=1)
            print(f"Sent 'generate_trajectory' command with Request ID: {req_id}")
        elif cmd == 's':
            command_payload = {
                "command": "stop_trajectory",
                "request_id": "N/A" 
            }
            client.publish(COMMAND_TOPIC, json.dumps(command_payload), qos=1)
            print("Sent 'stop_trajectory' command.")
        else:
            print("Invalid command.")

    client.loop_stop()
    client.disconnect()
    print("Subscriber stopped.")
